#ifndef VEL_JSON_HPP
#define VEL_JSON_HPP

#include <jsoncpp/json/json.h>
#include <string>
#include <iostream>
#include <memory> 

namespace line_alignment{

    class VelocityJsonConverter
    {
    public:
        VelocityJsonConverter(){}
        ~VelocityJsonConverter(){}
        
        double linear_x, linear_y, angular_z;  

        std::string json_msg;

        // 设置速度值
        void set_from_twist(double lx, double ly, double az);

        // 生成并返回JSON字符串
        std::string get_json_string() const;

        // 从JSON字符串解析数据
        bool parse_json_string(const std::string& json_str);

        // 获取当前存储的JSON消息
        const std::string& get_json_msg() const { return json_msg; }


        // 获取速度值
        double get_linear_x() const { return linear_x; }
        double get_linear_y() const { return linear_y; }
        double get_angular_z() const { return angular_z; }

        
    };


    inline void VelocityJsonConverter::set_from_twist(double lx, double ly, double az)
    {
        linear_x = lx;
        linear_y = ly;
        angular_z = az;
    }


    inline std::string VelocityJsonConverter::get_json_string() const 
    {

        Json::Value json_val;
        json_val["linear_x"] = linear_x;
        json_val["linear_y"] = linear_y;
        json_val["angular_z"] = angular_z;

        Json::StreamWriterBuilder builder;
        builder["indentation"] = ""; // 紧凑格式
        std::string result = Json::writeString(builder, json_val);

        return result;
        
    }

    inline bool VelocityJsonConverter::parse_json_string(const std::string& json_str)
    {
        Json::Value json_val;
        Json::CharReaderBuilder reader_builder;
        std::string errors;

        std::unique_ptr<Json::CharReader> reader(reader_builder.newCharReader());
        
        if (!reader->parse(json_str.c_str(), json_str.c_str() + json_str.length(), &json_val, &errors)) {
            std::cerr << "JSON parse error: " << errors << std::endl;
            return false;
        }

        try {
            linear_x = json_val["linear_x"].asDouble();
            linear_y = json_val["linear_y"].asDouble();
            angular_z = json_val["angular_z"].asDouble();
            
            // 更新json_msg
            json_msg = json_str;
            return true;
        } catch (const std::exception& e) {
            std::cerr << "Error accessing JSON values: " << e.what() << std::endl;
            return false;
        }
    }
}



#endif